#ifndef COOLPOOL_SIMULATION_SIMOBJECTS_BALLCOLLISIONDETECTORS
#define COOLPOOL_SIMULATION_SIMOBJECTS_BALLCOLLISIONDETECTORS

#include "BallCollisionDetectors.h"

namespace coolpool
{
	Collision * BallBallCollisionDetector::detect(Entity * ball1, Entity * ball2) const throw (UnexpectedInputException)
	{
		// Check for interpenetration
		if (ball1->getType() != CP_BALL || ball2->getType() != CP_BALL)
			throw UnexpectedInputException();

		Ball * ball1_c = static_cast<Ball *> (ball1);
		Ball * ball2_c = static_cast<Ball *> (ball2);
		ball1_c->getCenterOfMassLocation();
		double dist_sqr = (ball1_c->getCenterOfMassLocation() - ball2_c->getCenterOfMassLocation()).magSqr();
		double min_dist_sqr = math_tools::sqr(ball1_c->getRadius() + ball2_c->getRadius());

		// If they do not interpenetrate return NULL.
		if (dist_sqr > min_dist_sqr)
			return NULL;

		// If true calculate the collision properties
		Vector3D collision_normal = (ball1_c->getCenterOfMassLocation() - ball2_c->getCenterOfMassLocation()).unit();
		Vector3D collision_point = ball2_c->getCenterOfMassLocation() + collision_normal * ball2_c->getRadius();
		double vel_rel = collision_normal.dot(ball1_c->pointVelocity(collision_point) - ball2_c->pointVelocity(collision_point));

		Collision * col = new Collision;
		setCollisionType(vel_rel, col);
		col->first = static_cast<DynamicEntity *> (ball1);
		col->second = ball2;
		col->collision_normal = collision_normal;
		col->collision_point = collision_point;
		return col;
	}

	Collision * BallRectangleCollisionDetector::detect(Entity * ball, Entity * rectangle) const throw (UnexpectedInputException)
	{
		if (rectangle->getType() != CP_REC || ball->getType() != CP_BALL)
			throw UnexpectedInputException();

		// Check for interpenetration.
		Rectangle * rectangle_c = static_cast<Rectangle *> (rectangle);
		Ball * ball_c = static_cast<Ball *> (ball);

		// Check boundaries.
		if (!rectangle_c->isInRec(ball_c->getCenterOfMassLocation()))
			return NULL;

		// If they do not interpenetrate return NULL.
		double dist_sqr = rectangle_c->distanceFromSqr(ball_c->getCenterOfMassLocation());
		if (dist_sqr > math_tools::sqr(ball_c->getRadius()))
			return NULL;

		Vector3D collision_normal = rectangle_c->getPlane().getNormal();
		Vector3D collision_point = ball_c->getCenterOfMassLocation() - (collision_normal*ball_c->getRadius());
		double vel_rel = collision_normal.dot(ball_c->pointVelocity(collision_point));

		Collision * col = new Collision;
		col->first = static_cast<DynamicEntity *> (ball);
		col->second = rectangle;
		col->collision_normal = collision_normal;
		col->collision_point = collision_point;
		setCollisionType(vel_rel, col);
		return col;
	}
}

#endif
